package com.jmex.physics.impl.bullet;

import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import java.util.logging.Level;
import java.util.logging.Logger;

import com.jme.math.Vector3f;
import com.jmex.physics.CollisionGroup;
import com.jmex.physics.Joint;
import com.jmex.physics.PhysicsNode;
import com.jmex.physics.PhysicsSpace;
import com.jmex.physics.PhysicsSpatial;
import com.jmex.physics.geometry.PhysicsBox;
import com.jmex.physics.geometry.PhysicsCapsule;
import com.jmex.physics.geometry.PhysicsCylinder;
import com.jmex.physics.geometry.PhysicsMesh;
import com.jmex.physics.geometry.PhysicsSphere;
import com.jmex.physics.impl.bullet.geometry.BulletBox;
import com.jmex.physics.impl.bullet.geometry.BulletCapsule;
import com.jmex.physics.impl.bullet.geometry.BulletCylinder;
import com.jmex.physics.impl.bullet.geometry.BulletMesh;
import com.jmex.physics.impl.bullet.geometry.BulletSphere;
import com.jmex.physics.impl.bullet.joints.BulletJoint;

public class BulletPhysicsSpace extends PhysicsSpace {

    private List<PhysicsNode> physicsNodes = new ArrayList<PhysicsNode>();
    private List<BulletJoint> joints = new ArrayList<BulletJoint>();

    private BulletCollisionGroup staticCollisionGroup;
	private BulletCollisionGroup defaultCollisionGroup;
	
	private Vector3f tempVec1 = new Vector3f();
	private Vector3f tempVec2 = new Vector3f();
	private Vector3f tempVec3 = new Vector3f();
	
	private List<float[]> updates = new ArrayList<float[]>();
	private List<Long> updateHandles = new ArrayList<Long>();

	private List<float[]> forces = new ArrayList<float[]>();
	private List<Long> forceHandles = new ArrayList<Long>();
	
	private float accuracy = 1f/60f;
	
	protected BulletPhysicsSpace()
	{
		BulletNativeInvoker.createPhysicsWorld(this);

		staticCollisionGroup = createCollisionGroup( "static" );
        defaultCollisionGroup = createCollisionGroup( "default" );
        staticCollisionGroup.collidesWith(defaultCollisionGroup, true);
	}
	
    static class BulletFactory implements Factory {
        public BulletFactory() {
            setFactory( this );
        }

        public PhysicsSpace create() {
            BulletPhysicsSpace space = new BulletPhysicsSpace();
            return space;
        }

        public String getImplementationName() {
            return "Bullet";
        }

        public String getImplementationVersion() {
            return "2.75";
        }
    }

    @Override
    public void addJoint( Joint joint ) {
        super.addJoint( joint );

        // log some stuff:
        Logger logger = Logger.getLogger( PhysicsSpace.LOGGER_NAME );
        if ( logger.isLoggable( Level.INFO ) ) {
            logger.log( Level.INFO,
                    "Joint (" + joint.getName() + ") has been added" );
        }

        // add it to the arraylist
        joints.add( (BulletJoint)joint );
    }

    protected void removeJoint( Joint joint ) {

        if ( !joints.remove( joint ) ) {
            return;
        }
        super.removeJoint( joint );

        // print out a statement
        Logger logger = Logger.getLogger( PhysicsSpace.LOGGER_NAME );
        if ( logger.isLoggable( Level.INFO ) ) {
            logger.log( Level.INFO,
                    "Joint ("
                            + joint.getName()
                            + ") has been removed from PhysicsWorld and will no longer take place in the simulation" );
        }
    }

    @Override
    public void addNode( PhysicsNode node ) {
        super.addNode( node );

        // log some stuff:
        Logger logger = Logger.getLogger( PhysicsSpace.LOGGER_NAME );
        if ( logger.isLoggable( Level.INFO ) ) {
            logger.log( Level.INFO,
                    "PhysicsNode (" + node.getName() + ") has been added" );
        }

        // add it to the arraylist
        physicsNodes.add( node );
    }

    protected void removeNode( PhysicsNode obj ) {

        if ( !physicsNodes.remove( obj ) ) {
            return;
        }
        super.removeNode( obj );

        // print out a statement
        Logger logger = Logger.getLogger( PhysicsSpace.LOGGER_NAME );
        if ( logger.isLoggable( Level.INFO ) ) {
            logger.log( Level.INFO,
                    "PhysicsObject ("
                            + obj.getName()
                            + ") has been removed from PhysicsWorld and will no longer take place in the simulation" );
        }
    }

	@Override
	public boolean collide(PhysicsSpatial spatial1, PhysicsSpatial spatial2) {
		// TODO Auto-generated method stub
		return false;
	}

	@Override
	public BulletCollisionGroup createCollisionGroup(String name) {
		return new BulletCollisionGroup(this,name);
	}

    @Override
    protected PhysicsBox createBox( String name, PhysicsNode node ) {
        BulletBox ret = new BulletBox( node );
        ret.setName(name);
        if ( node != null ) {
            node.attachChild( ret );
        }
        return ret;
    }

    @Override
    protected PhysicsSphere createSphere( String name, PhysicsNode node ) {
        BulletSphere ret = new BulletSphere( node );
        ret.setName(name);
        if ( node != null ) {
            node.attachChild( ret );
        }
        return ret;
    }
    
	@Override
    protected PhysicsCylinder createCylinder( String name, PhysicsNode node ) {
        BulletCylinder ret = new BulletCylinder( node );
        ret.setName(name);
        if ( node != null ) {
            node.attachChild( ret );
        }
        return ret;
    }

    @Override
    protected PhysicsCapsule createCapsule( String name, PhysicsNode node ) {
        BulletCapsule ret = new BulletCapsule( node );
        ret.setName(name);
        if ( node != null ) {
            node.attachChild( ret );
        }
        return ret;
    }

	@Override
	protected PhysicsMesh createMesh(String name, PhysicsNode node) {
        BulletMesh ret = new BulletMesh( node );
        ret.setName(name);
        if ( node != null ) {
            node.attachChild( ret );
        }
        return ret;
	}

	@Override
    public BulletDynamicPhysicsNode createDynamicNode() {
        BulletDynamicPhysicsNode node = new BulletDynamicPhysicsNode( this );
        node.setCollisionGroup( defaultCollisionGroup );
        node.setActive( true );
        return node;
    }

    @Override
    public BulletJoint createJoint() {
        BulletJoint ret = new BulletJoint( this );
        addJoint( ret );
        return ret;
    }

    @Override
    public BulletStaticPhysicsNode createStaticNode() {
        BulletStaticPhysicsNode node = new BulletStaticPhysicsNode( this );
        node.setCollisionGroup( staticCollisionGroup );
        node.setActive( true );
        return node;
    }

	@Override
	public void delete() {
		// TODO Auto-generated method stub

	}

	@Override
	public CollisionGroup getDefaultCollisionGroup() {
		return defaultCollisionGroup;
	}

	@Override
	public Vector3f getDirectionalGravity(Vector3f store) {
		// TODO Auto-generated method stub
		return null;
	}

	@Override
	public List<? extends Joint> getJoints() {
        return Collections.unmodifiableList( joints );
	}

    @Override
    public List<? extends PhysicsNode> getNodes() {
        return Collections.unmodifiableList( physicsNodes );
    }

	@Override
	public CollisionGroup getStaticCollisionGroup() {
		return staticCollisionGroup;
	}

	@Override
	public void pick(PhysicsSpatial spatial) {
		// TODO Auto-generated method stub

	}

	@Override
	public void setAccuracy(float value) {
		accuracy = value;
	}

	@Override
	public void setAutoRestThreshold(float threshold) {
		// TODO Auto-generated method stub

	}

	@Override
	public void setDirectionalGravity(Vector3f gravity) {
		// TODO Auto-generated method stub

	}

	@Override
	public void update(float time) {
		for(PhysicsNode node : physicsNodes)
		{
			if(node.isStatic() && ((BulletStaticPhysicsNode)node).isDirty())
				((BulletStaticPhysicsNode)node).rebuildRigidBody();
			if(!node.isStatic() && ((BulletDynamicPhysicsNode)node).isDirty())
				((BulletDynamicPhysicsNode)node).rebuildRigidBody();
		}
		
		for(Joint joint : joints)
		{
			if(((BulletJoint)joint).isDirty())
				((BulletJoint)joint).buildConstraint();
			((BulletJoint)joint).updateJointAxes();
		}
		
		updateHandles.clear();
		updates.clear();
		forceHandles.clear();
		forces.clear();
		for(PhysicsNode node : physicsNodes)
		{
			if(node.isStatic() && ((BulletStaticPhysicsNode)node).isTouched())
			{
				updates.add(new float[]{node.getWorldTranslation().x,node.getWorldTranslation().y,node.getWorldTranslation().z,
										node.getWorldRotation().x,node.getWorldRotation().y,node.getWorldRotation().z,node.getWorldRotation().w,
										0,0,0,
										0,0,0});
				updateHandles.add(((BulletStaticPhysicsNode)node).getBody().getNativeBulletHandle());
			}
			if(!node.isStatic())
			{		
				if(((BulletDynamicPhysicsNode)node).isTouched())
				{
					((BulletDynamicPhysicsNode)node).getLinearVelocity(tempVec1);
					((BulletDynamicPhysicsNode)node).getAngularVelocity(tempVec2);
					((BulletDynamicPhysicsNode)node).getCenterOfMass(tempVec3);
					node.getWorldRotation().mult(tempVec3, tempVec3);
					tempVec3.addLocal(node.getWorldTranslation());
					updates.add(new float[]{tempVec3.x,tempVec3.y,tempVec3.z,
											node.getWorldRotation().x,node.getWorldRotation().y,node.getWorldRotation().z,node.getWorldRotation().w,
											tempVec1.x,tempVec1.y,tempVec1.z,
											tempVec2.x,tempVec2.y,tempVec2.z});
					updateHandles.add(((BulletDynamicPhysicsNode)node).getBody().getNativeBulletHandle());
				}
				if(((BulletDynamicPhysicsNode)node).getAppliedForces().size()>0)
				{
					for(Vector3f[] storedForces : ((BulletDynamicPhysicsNode)node).getAppliedForces())
					{
						float[] temp = new float[9];
						if(storedForces[0]!=null)
						{
							temp[0]=storedForces[0].x;
							temp[1]=storedForces[0].y;
							temp[2]=storedForces[0].z;
						} else {
							temp[0]=Float.NaN;
							temp[1]=Float.NaN;
							temp[2]=Float.NaN;
						}
						if(storedForces[1]!=null)
						{
							temp[3]=storedForces[1].x;
							temp[4]=storedForces[1].y;
							temp[5]=storedForces[1].z;
						} else {
							temp[3]=Float.NaN;
							temp[4]=Float.NaN;
							temp[5]=Float.NaN;
						}
						if(storedForces[2]!=null)
						{
							temp[6]=storedForces[2].x;
							temp[7]=storedForces[2].y;
							temp[8]=storedForces[2].z;
						} else {
							temp[6]=Float.NaN;
							temp[7]=Float.NaN;
							temp[8]=Float.NaN;
						}
						forces.add(temp);
						forceHandles.add(((BulletDynamicPhysicsNode)node).getBody().getNativeBulletHandle());
					}
				}
			}
			if(!node.isStatic())
			{
				((BulletDynamicPhysicsNode)node).sync();
				((BulletDynamicPhysicsNode)node).getAppliedForces().clear();
			}
			else
				((BulletStaticPhysicsNode)node).sync();
		}
		if(updates.size()>0)
		{
			long[] handles = new long[updates.size()];
			float[] tx = new float[updates.size()];
			float[] ty = new float[updates.size()];
			float[] tz = new float[updates.size()];
			float[] rx = new float[updates.size()];
			float[] ry = new float[updates.size()];
			float[] rz = new float[updates.size()];
			float[] rw = new float[updates.size()];
			float[] lvx = new float[updates.size()];
			float[] lvy = new float[updates.size()];
			float[] lvz = new float[updates.size()];
			float[] avx = new float[updates.size()];
			float[] avy = new float[updates.size()];
			float[] avz = new float[updates.size()];
			int index=0;
			for(float[] change : updates)
			{
				handles[index]=updateHandles.get(index);
				tx[index]=change[0];
				ty[index]=change[1];
				tz[index]=change[2];
				rx[index]=change[3];
				ry[index]=change[4];
				rz[index]=change[5];
				rw[index]=change[6];
				lvx[index]=change[7];
				lvy[index]=change[8];
				lvz[index]=change[9];
				avx[index]=change[10];
				avy[index]=change[11];
				avz[index]=change[12];
				index++;
			}
			BulletNativeInvoker.pushRigidBodyInfo(handles, 
					tx, ty, tz, 
					rx, ry, rz, rw, 
					lvx, lvy, lvz, 
					avx, avy, avz);
		}
		if(forces.size()>0)
		{
			long[] handles = new long[forces.size()];
			float[] fx = new float[forces.size()];
			float[] fy = new float[forces.size()];
			float[] fz = new float[forces.size()];
			float[] px = new float[forces.size()];
			float[] py = new float[forces.size()];
			float[] pz = new float[forces.size()];
			float[] tx = new float[forces.size()];
			float[] ty = new float[forces.size()];
			float[] tz = new float[forces.size()];
			int index=0;
			for(float[] entry : forces)
			{
				handles[index]=forceHandles.get(index);
				fx[index]=entry[0]*((time<accuracy)?accuracy:time);
				fy[index]=entry[1]*((time<accuracy)?accuracy:time);
				fz[index]=entry[2]*((time<accuracy)?accuracy:time);
				px[index]=entry[3]*((time<accuracy)?accuracy:time);
				py[index]=entry[4]*((time<accuracy)?accuracy:time);
				pz[index]=entry[5]*((time<accuracy)?accuracy:time);
				tx[index]=entry[6]*((time<accuracy)?accuracy:time);
				ty[index]=entry[7]*((time<accuracy)?accuracy:time);
				tz[index]=entry[8]*((time<accuracy)?accuracy:time);
				index++;
			}
			BulletNativeInvoker.applyForces(handles,
					fx,fy,fz,
					px,py,pz,
					tx,ty,tz);
		}
		
		BulletNativeInvoker.step(this,time);
	}
	
	public void applyPhysicsMovement(BulletRigidBody[] rbList,
									 double[] tx,double[] ty,double[] tz,
									 double[] rx,double[] ry,double[] rz,double[] rw)
	{
		for(int loop=0; loop<rbList.length; loop++)
		{
			rbList[loop].applyPhysicsMovement(tx[loop], ty[loop], tz[loop], rx[loop], ry[loop], rz[loop], rw[loop]);
		}
	}
}
